import numpy as np
import matplotlib.pyplot as plt
'''
概述：列举100个时间点，从第二个时间点开始，已知上个时间点的位置和速度，粗略估计出当前时间点的位置，
再利用卡尔曼滤波算法，融合观测得到的当前位置，得出当前更精准的位置。
'''

# 模拟数据，有100个时间点，1，2，3...100，然后算出每个样本时间点的测量位置
t = np.linspace(1, 100, 100)
print(t)
a = 0.5  #加速度为a
position = (a * t ** 2) / 2   #根据加速度与位移关系，算出样本各时间点测量的距离
print(position)
'''
假设雷达测量时噪音服从正态分布N(0,120^2),标准差为120
根据均值和标准差来估计导弹的真实值的范围
每个时刻用雷达测量出导弹位置为position_noise
'''
position_noise_std = 120   #测量时噪音的标准差
position_noise = position + np.random.normal(0, position_noise_std, size=(t.shape[0]))


# plt.plot(t, position, label='truth position')

# 雷达测量值各时刻位置为y轴，时间为x轴画出图像
plt.plot(t, position_noise, label='measured position')

# 初始的估计导弹的位置就直接用雷达测量的位置
predicts = [position_noise[0]]
position_predict = predicts[0]

predict_var = 0
odo_var = position_noise_std**2  # 测量时噪音的方差，越大则测量值占比越低
v_std = 50  # 测量仪器的标准差
'''
从第二个时间点，以此根据上个时间点的【距离】和【速度】算出当前时间的粗略位置
根据均值和标准差来估计导弹的真实值的范围
'''
for i in range(1, t.shape[0]):
    dv = (position[i] - position[i - 1]) + np.random.normal(0, 50)
    position_predict = position_predict + dv  # 利用上个时刻的位置和速度预测当前位置
    predict_var += v_std ** 2  # 更新预测数据的方差
    '''
    下面是Kalman filter
    position_predict服从正态分布：
    期望μ = position_predict * odo_var / (predict_var + odo_var) + position_noise[i] * predict_var / (predict_var + odo_var)
    方差σ^2 = (predict_var * odo_var)/ (predict_var + odo_var)^2
    '''
    position_predict = position_predict * odo_var / (predict_var + odo_var) + position_noise[i] * predict_var / (predict_var + odo_var)
    predict_var = (predict_var * odo_var) / (predict_var + odo_var) ** 2
    predicts.append(position_predict)
plt.plot(t, predicts, 'r', label='kalman filtered position')
plt.xlabel('Time')
plt.ylabel('Position')
plt.grid(alpha=0.2)
plt.legend()
plt.show()